﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.IO.MemoryMappedFiles;
using System.Threading;
using System.Diagnostics;
using System.IO;

namespace CANConverter
{
    public partial class Main : Form
    {
        MemoryMappedFile memFile;
        MemoryMappedViewAccessor mva;
        double Counter;
        static uint canHandle;
        Thread RXThread;
        bool RXActive;
        Thread TXThread;
        bool TXActive;

        // rate
        Stopwatch RateSW = new Stopwatch();
        int LastRXCount;
        int LastTXCount;
        double FiltRXRate;
        double FiltTXRate;

        // data
        SDataStructure DataStructure = new SDataStructure();

        public struct SDataStructure
        {
            public int ReceivedPackets;
            public int ReceiveErrors;
            public int RXThreadTicks;

            public int TransmitPackets;
            public int TransmitErrors;
            public int TXThreadTicks;
        }

        public enum EType { Float, Double, FloatWithTick };

        public class SCANMsgDescr
        {
            public uint ID;
            public EType Type;
            public int Frequency;
            public double TimeToSend;
            public int Count;
        }

        List<SCANMsgDescr> CANDescriptorList = new List<SCANMsgDescr>();

        public Main()
        {
            InitializeComponent();

            InitMemoryFile();

            RateSW.Start();
            
            // fill list
            // IOBoard
            AddData(0x10120, EType.Float, 0); // PWM1
            AddData(0x10121, EType.Float, 0); // PWM2
            AddData(0x10122, EType.Float, 0); // PWM3
            AddData(0x10123, EType.Float, 0); // PWM4
            AddData(0x10300, EType.Float, 100); // Sonic [m]?
            AddData(0x10311, EType.Float, 10); // Voltage [V]
            
            // IMU Board
            AddData(0x30300, EType.Float, 100); // GyroX
            AddData(0x30301, EType.Float, 100); // GyroY
            AddData(0x30302, EType.Float, 100); // GyroZ
            AddData(0x30306, EType.Float, 100);  // MagX
            AddData(0x30307, EType.Float, 100);  // MagY
            AddData(0x30308, EType.Float, 100);  // MagZ
            AddData(0x30310, EType.Float, 100); // Roll
            AddData(0x30311, EType.Float, 100); // Pitch
            //AddData(0x30312, EType.Float, 100); // Yaw
            AddData(0x30320, EType.Float, 100); // PressureAbs
            AddData(0x30330, EType.Double, 5); // Longitude
            AddData(0x30331, EType.Double, 5); // Latitude
            AddData(0x30332, EType.Float, 5); // GPS Alt
            AddData(0x30333, EType.Float, 5); // GPS Sat Nr

            // XBee Board
            AddData(0x40350, EType.Float, 0);
            AddData(0x40351, EType.Double, 0);
            AddData(0x40352, EType.Float, 0);
        }

        public void AddData(uint id, EType type, int frequency)
        {
            SCANMsgDescr descr = new SCANMsgDescr();
            descr.ID = id;
            descr.Type = type;
            descr.Frequency = frequency;
            descr.TimeToSend = 0;
            descr.Count = 0;

            CANDescriptorList.Add(descr);

            // add to listView
            ListViewItem item = new ListViewItem(string.Format("0x{0:X}", id));
            item.Name = id.ToString(); // KEY
            item.SubItems.Add("0");
            item.SubItems.Add(type.ToString());
            item.SubItems.Add(frequency.ToString());
            item.SubItems.Add("0");
            listViewCANMsgs.Items.Add(item);
        }

        private void Main_Load(object sender, EventArgs e)
        {
            toolStripStatusLabelCANAdapter.Text = "No adapters";

            // find adapters
            StringBuilder buf = new StringBuilder(32);
            int cnt;

            // Fill the listbox with data
            cnt = LAWICEL.canusb_getFirstAdapter(buf, 32);
            if (cnt > 0) toolStripStatusLabelCANAdapter.Text = "Adapter found: " + buf.ToString();
        }

        private void InitMemoryFile()
        {
            memFile = MemoryMappedFile.CreateNew("myCANFile", int.MaxValue);
            mva = memFile.CreateViewAccessor(0, 0x050000*10);
        }

        private void timer1_Tick(object sender, EventArgs e)
        {
            // test Counter
            //Counter++;
            mva.Write(0, Counter);
            textBoxCounter.Text = string.Format("{0}", Counter);

            // update GUI
            // 1. get copy
            SDataStructure copyStruct;
            lock (this)
            {
                copyStruct = DataStructure;
            }

            // 2. set values
            textBoxTXRXFrames.Text = string.Format("{0}/{1}", copyStruct.TransmitPackets, copyStruct.ReceivedPackets);
            textBoxTXRXErrors.Text = string.Format("{0}/{1}", copyStruct.TransmitErrors, copyStruct.ReceiveErrors);
            textBoxTXRXTicks.Text = string.Format("{0}/{1}", copyStruct.TXThreadTicks, copyStruct.RXThreadTicks);

            // calclate rates
            double deltaT = (double)RateSW.ElapsedTicks / (double)Stopwatch.Frequency;
            RateSW.Restart();
            double txRate = (copyStruct.TransmitPackets - LastTXCount) / deltaT;
            double rxRate = (copyStruct.ReceivedPackets - LastRXCount) / deltaT;
            LastTXCount = copyStruct.TransmitPackets;
            LastRXCount = copyStruct.ReceivedPackets;
            FiltRXRate = FiltRXRate * 0.95 + rxRate * 0.05;
            FiltTXRate = FiltTXRate * 0.95 + txRate * 0.05;
            textBoxRXRate.Text = string.Format("{0} pkt/sec", (int)FiltRXRate);
            textBoxTXRate.Text = string.Format("{0} pkt/sec", (int)FiltTXRate);

            // update all MSGs
            listViewCANMsgs.BeginUpdate();
            foreach (SCANMsgDescr descr in CANDescriptorList)
            {
                uint index = descr.ID * 10; // 10 bytes perID
                string key = descr.ID.ToString();
                if (descr.Type == EType.Float)
                {
                    float value = mva.ReadSingle(index);
                    listViewCANMsgs.Items[key].SubItems[1].Text = value.ToString();
                }
                if (descr.Type == EType.Double)
                {
                    double value = mva.ReadDouble(index);
                    listViewCANMsgs.Items[key].SubItems[1].Text = value.ToString();
                }
                if (descr.Type == EType.FloatWithTick)
                {
                    float value = mva.ReadSingle(index);
                    UInt32 tick = mva.ReadUInt32(index + 4);
                    listViewCANMsgs.Items[key].SubItems[1].Text = string.Format("{0}/{1}", value, tick);
                }
                listViewCANMsgs.Items[key].SubItems[4].Text = string.Format("{0}", descr.Count);
            }
            listViewCANMsgs.EndUpdate();
        }

        private void buttonOpen_Click(object sender, EventArgs e)
        {
            canHandle = LAWICEL.canusb_Open(IntPtr.Zero,
                                       LAWICEL.CAN_BAUD_1M,
                                       LAWICEL.CANUSB_ACCEPTANCE_CODE_ALL,
                                       LAWICEL.CANUSB_ACCEPTANCE_MASK_ALL,
                                       0);

            if (canHandle == 0) MessageBox.Show("Open Failed");
            else
            {
                // OK
                buttonOpen.Enabled = false;
                buttonClose.Enabled = true;

                // init structures
                DataStructure = new SDataStructure();

                // create receive thread
                RXActive = true;
                RXThread = new Thread(new ThreadStart(RXThreadProc));
                //RXThread.Priority = ThreadPriority.AboveNormal;
                RXThread.IsBackground = true;
                RXThread.Start();

                // create transmitt thread
                TXActive = true;
                TXThread = new Thread(new ThreadStart(TXThreadProc));
                //RXThread.Priority = ThreadPriority.AboveNormal;
                TXThread.IsBackground = true;
                TXThread.Start();
            }
        }

        private void buttonClose_Click(object sender, EventArgs e)
        {
            RXActive = false;

            // Close the CANUSB
            int res = LAWICEL.canusb_Close(canHandle);
            if (LAWICEL.ERROR_CANUSB_OK == res)
            {
                // OK
                buttonOpen.Enabled = true;
                buttonClose.Enabled = false;    
            }
            else
            {
                MessageBox.Show("Close Failed");
            }
        }

        void RXThreadProc()
        {
            StreamWriter writer = File.CreateText("logfile.txt");
            writer.WriteLine("CANMsg Value Tick");

            while (RXActive)
            {
                // Read one CAN message
                LAWICEL.CANMsg msg = new LAWICEL.CANMsg();

                int rv = LAWICEL.canusb_Read(canHandle, out msg);
                if (LAWICEL.ERROR_CANUSB_OK == rv)
                {
                    // data OK, convert to float
                    byte[] bytes = BitConverter.GetBytes(msg.data);
                    

                    // XXX - log to file
                    float data = BitConverter.ToSingle(bytes, 0);
                    //double dataDbl = BitConverter.ToDouble(bytes, 0);
                    UInt32 tick = BitConverter.ToUInt32(bytes, 4);
                    string line = string.Format("{0} {1} {2}", msg.id, data, tick);
                    writer.WriteLine(line);
                    // XXX - done

                    lock (this)
                    {
                        // put to sharedmemory
                        long index = msg.id * 10; // 10 bytes perID
                        mva.WriteArray(index, bytes, 0, msg.len);

                        DataStructure.ReceivedPackets++;
                    }
                }
                else if (LAWICEL.ERROR_CANUSB_NO_MESSAGE == rv)
                {
                    // no messages, sleep
                    Thread.Sleep(1);
                    lock (this)
                    {
                        DataStructure.RXThreadTicks++;
                    }
                }
                else
                {
                    // error!
                    lock (this)
                    {
                        DataStructure.ReceiveErrors++;
                    }
                }
            }
            writer.Close();
        }

        void TXThreadProc()
        {
            Stopwatch sw = new Stopwatch();
            sw.Start();
            while (TXActive)
            {
                Thread.Sleep(10); // XXX -> comment for multicore CPUs
                double deltaT = (double)sw.ElapsedTicks/(double)Stopwatch.Frequency;
                sw.Restart();
                Counter += deltaT;

                // check all msgs
                foreach (SCANMsgDescr descr in CANDescriptorList)
                {
                    uint index = descr.ID * 10; // 10 bytes perID
                    string key = descr.ID.ToString();

                    if (descr.Frequency > 0)
                    {
                        // calculate timestamp
                        descr.TimeToSend -= deltaT;
                        if (descr.TimeToSend < 0)
                        {
                            // send data
                            descr.TimeToSend = 1.0 / descr.Frequency;
                            LAWICEL.CANMsg msg = new LAWICEL.CANMsg();
                            msg.id = descr.ID;
                            msg.len = 8;
                            msg.flags = LAWICEL.CANMSG_EXTENDED;
                            byte[] data = new byte[8];
                            if (descr.Type == EType.Float)
                            {
                                float value = mva.ReadSingle(index);
                                BitConverter.GetBytes(value).CopyTo(data, 0);
                            }
                            if (descr.Type == EType.Double)
                            {
                                double value = mva.ReadDouble(index);
                                BitConverter.GetBytes(value).CopyTo(data, 0);
                            }
                            
                            msg.data = BitConverter.ToUInt64(data, 0);
                            int rv = LAWICEL.canusb_Write(canHandle, ref msg);
                            if (LAWICEL.ERROR_CANUSB_OK == rv)
                            {
                                descr.Count++;
                                lock (this)
                                {
                                    DataStructure.TransmitPackets++;
                                }
                            }
                            else
                            {
                                // error!
                                lock (this)
                                {
                                    DataStructure.TransmitErrors++;
                                }
                            }           
                        }
                    }
                }
                lock (this)
                {
                    DataStructure.TXThreadTicks++;
                }
            }
        }
    }
}
